#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import sys
import time   
import socket
import binascii
import goto
from goto import with_goto
from robot_pkg.msg import Person1



class TCP232(object):

	def __init__(self):
		self.angle_x=0
		self.angle_y=0
		self.angle_z=0
		self.pressure = 0#压力
		self.longitude =0#经度
		self.latitude = 0#纬度
		self.Voltage = 0	 #电压
		self.Current = 0	 #电流
		self.Coulomb = 0	 #剩余容量
		self.Battery = 0	 #容量百分比
		self.Temperature1 = 0	 #温度1
		self.Temperature2 = 0	 #温度2
		self.Temperature3 = 0	 #温度3
		# 服务器地址和端口
		host = "192.168.1.21"
		port = 8888
		# 创建一个socket对象
		self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		#设置读取超时时间
		self.client_socket.settimeout(1)
		# 连接目标服务器
		while(True):
			time.sleep(1)
			try:
				self.socket_client = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
				res=self.client_socket.connect((host,port))
				time.sleep(0.1)
				if not res:
					break
			except:
				pass

	#@with_goto
	def get_data(self):
		#获取陀螺仪数据
		message1 = b"\x50\x03\x00\x3d\x00\x03\x99\x86"
		self.client_socket.sendall(message1)
		try:
			response_angle = self.client_socket.recv(1024)
			response_hex_angle = binascii.hexlify(response_angle).decode("utf-8")
			if(len(response_hex_angle)>=18):
				self.angle_x=int(response_hex_angle[6:10],16)
				self.angle_y=int(response_hex_angle[10:14],16)
				self.angle_z=int(response_hex_angle[14:18],16)
		except socket.timeout:
			time.sleep(0.5)


		#获取液位变送器数据
		#message2 = b"\x01\x03\x00\x04\x00\x01\xc5\xcb"
		#self.client_socket.sendall(message2)
		#response_pressure = self.client_socket.recv(1024)
		#response_hex_pressure = binascii.hexlify(response_pressure).decode("utf-8")
		#self.pressure = int(response_hex_pressure[6:10],16)
		
		#获取GPS数据
		message3 = b"\x01\x03\x00\x18\x00\x06\x45\xcf"
		self.client_socket.sendall(message3)
		try:
			response_gps = self.client_socket.recv(1024)
			response_hex_gps = binascii.hexlify(response_gps).decode("utf-8")
			if(len(response_hex_gps)>=30):
				self.longitude = int(response_hex_gps[10:18],16)
				self.latitude = int(response_hex_gps[22:30],16)
		except socket.timeout:
			time.sleep(0.5)
		# #获取库仑计数据
		message4 = b"\xdd\xa5\x03\x00\xff\xfd\x77"
		self.client_socket.sendall(message4)
		try:
			response_coulomb = self.client_socket.recv(1024)
			response_hex = binascii.hexlify(response_coulomb).decode("utf-8")
			if(len(response_hex)>8):
				Ster = int(response_hex[4:6],16)	 #状态
				datalen = int(response_hex[6:8],16)	 #数据长度
				if(Ster == 0):
					if(datalen >= 29):
						self.Voltage = int(response_hex[8:12],16)	 #电压
						#电流充电为正，放电为负
						self.Current = int(response_hex[12:16],16)	 #电流
						if(int(response_hex[12:13],16) > 7):	 #16进制第一个数大于7，则为负
							self.Current = 0xffff - self.Current + 1
						self.Coulomb = int(response_hex[16:20],16)	 #剩余容量
						self.Battery = int(response_hex[46:48],16)	 #容量百分比
						NTC = int(response_hex[48:50],16)	 
						self.Temperature1 = int(response_hex[54:58],16) - 2731	 #温度1
						self.Temperature2 = int(response_hex[58:62],16) - 2731	 #温度2
						self.Temperature3 = int(response_hex[62:66],16) - 2731	 #温度3
		except socket.timeout:
			time.sleep(0.5)	
		# if(self.angle_x==0 and self.angle_y==0 and self.longitude==0 and self.latitude==0):
		# 	time.sleep(0.5)
			#goto.begin
	def tcp232_publisher(self):
		# ROS节点初始化
		rospy.init_node('tcp232_publisher', anonymous=True)

		person_info_pub = rospy.Publisher('/tcp232_publisher', Person1, queue_size=100)

		#设置循环的频率
		rate = rospy.Rate(10) 

		while not rospy.is_shutdown():
			self.get_data()
			# person_msg::Person类型的消息
			person_msg = Person1()
			# person_msg.angle_x=self.angle_x
			# person_msg.angle_y=self.angle_y
			if self.angle_z>=0:
				person_msg.angle_z=self.angle_z
			# person_msg.pressure=self.pressure
			# person_msg.longitude=self.longitude
			if self.latitude>=0:
				person_msg.latitude=self.latitude
			if self.Voltage>=0:
				person_msg.Voltage=self.Voltage
			if self.Current>=0:
				person_msg.Current=self.Current
			if self.Coulomb>=0:
				person_msg.Coulomb=self.Coulomb
			if self.Battery>=0:
				person_msg.Battery=self.Battery
			if self.Temperature1>=0:
				person_msg.Temperature1=self.Temperature1
				# person_msg.Temperature2=self.Temperature2
				# person_msg.Temperature3=self.Temperature3
			person_info_pub.publish(person_msg)
			rate.sleep()

if __name__ == '__main__':
    try:
        p = TCP232()
        p.tcp232_publisher()
    except rospy.ROSInterruptException:
        pass
